#include "Arduino.h"
#include "ITG3200.h"
#include "Wire.h"

ITG3200::ITG3200(void) {
    _gADDR = 0x69;   // defaults to ADO HIGH (D2/D3) 
                     // use .begin(0x68) for ADO LOW (D0/D1)
    _gGYRO = 0x1D;
    _gCSEL = 0x3E;
    _gXCLK = 0x01;
    _gDLPF = 0x16;
    _gSCBW = (0x03 << 3) + 0x01;  // Gain = 0x03 + BW = 1kHz = 0x01
    _x_off = 0;
    _y_off = 0;
    _z_off = 0;
}

void ITG3200::begin(void) {

  Wire.beginTransmission(_gADDR); // transmit to Gyro
  Wire.write(_gDLPF);        // Send Gain/BW register address
  Wire.write(_gSCBW);        // Set Gain & BW
  Wire.endTransmission();  // stop transmitting
  Wire.beginTransmission(_gADDR); // transmit to Gyro
  Wire.write(_gCSEL);        // Send Clock Select register address
  Wire.write(_gXCLK);        // Set clock to X-Gyro
  Wire.endTransmission();  // stop transmitting
}

void ITG3200::begin(int I2C_addr) {
  _gADDR = I2C_addr;
  begin();
}

void ITG3200::get_rates(void) {

  Wire.beginTransmission(_gADDR); // transmit to gyro
  Wire.write(_gGYRO);        // Send gyro data register address
  Wire.endTransmission();    // stop transmitting
  
  Wire.requestFrom(_gADDR, 6);

  byte MSB = Wire.read();
  x_gyro = (int(MSB) << 8) + Wire.read() + _x_off;
  
  MSB = Wire.read();
  y_gyro = (int(MSB) << 8) + Wire.read() + _y_off;
  
  MSB = Wire.read();
  z_gyro = (int(MSB) << 8) + Wire.read() + _z_off;

}

void ITG3200::set_offset(int num_samples) {

  long x_rate = 0;
  long y_rate = 0;
  long z_rate = 0;
  for (int ctr = 0; ctr < num_samples; ctr++) {
    get_rates();
    x_rate += x_gyro;
    y_rate += y_gyro;
    z_rate += z_gyro;
    delay(5);
  }
  
  x_rate = (x_rate << 1) + 1;
  y_rate = (y_rate << 1) + 1;
  z_rate = (z_rate << 1) + 1;
  
  _x_off -= int((x_rate / num_samples) >> 1);
  _y_off -= int((y_rate / num_samples) >> 1);
  _z_off -= int((z_rate / num_samples) >> 1);
}

void ITG3200::set_offset(int x_off, int y_off, int z_off) {
  _x_off -= x_off;
  _y_off -= y_off;
  _z_off -= z_off;
}
  

